#include <iostream>
#include "cmd_interface_linux.h"
#include <stdio.h>
#include "lipkg.h"
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include "transform.h"

#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)

int main(int argc, char **argv)
{
	ros::init(argc, argv, "ldldiar_publisher");

	ros::NodeHandle nh; // create a ROS Node
	ros::NodeHandle n("~/lidar");

	std::string product_name;
	std::string topic_name;
	std::string port_name;
	std::string frame_id;

	n.param<std::string>("product_name", product_name, "LD03");
	n.param<std::string>("topic_name", topic_name, "scan");
	n.param<std::string>("port_name", port_name, "/dev/ttyUSB0");
	n.param<std::string>("frame_id", frame_id, "lidar_link");

	LiPkg *pkg;

	int32_t ver = 0;

	if (product_name == "LD00")
	{
		ver = 0;
		pkg = new LD00_LiPkg;
	}
	else if (product_name == "LD03")
	{
		ver = 3;
		pkg = new LD03_LiPkg;
	}
	else if (product_name == "LD08")
	{
		ver = 8;
		pkg = new LD08_LiPkg;
	}
	else if (product_name == "LD09")
	{
		ver = 9;
		pkg = new LD09_LiPkg;
	}
	else
	{
		std::cout << "Usage : LD0* ->  LD00 LD03 LD08 LD09  " << std::endl; /* print Usage */
		return -1;
	}

	ros::Publisher lidar_pub = nh.advertise<sensor_msgs::LaserScan>(topic_name, 1); /*create a ROS topic */

	CmdInterfaceLinux cmd_port(ver);

	if (port_name.empty() == false)
	{
		cmd_port.SetReadCallback([&pkg](const char *byte, size_t len)
								 {
			if(pkg->Parse((uint8_t*)byte, len))
			{
				pkg->AssemblePacket();  
			} });
		cmd_port.Open(port_name);
		sensor_msgs::LaserScan scan;
		scan.header.stamp = ros::Time::now();
		scan.header.frame_id = frame_id;
		scan.range_min = 0.0;
		scan.range_max = 100.0;

		while (ros::ok())
		{
			if (pkg->IsFrameReady())
			{
				FrameData data = pkg->GetFrameData();
				scan.angle_min = ANGLE_TO_RADIAN(data.angle_min);
				scan.angle_max = ANGLE_TO_RADIAN(data.angle_max);
				scan.angle_increment = (scan.angle_max - scan.angle_min) / data.len;
				scan.ranges.resize(data.len);
				scan.intensities.resize(data.len);
				for (int i = 0; i < data.len; i++)
				{
					scan.ranges[i] = data.distance[i] / 1000.f;
					scan.intensities[i] = data.intensities[i];
				}
				scan.header.stamp = ros::Time::now();
				lidar_pub.publish(scan);
				// printf("min:%f,max:%f,len:%d\n", data.angle_min, data.angle_max, data.len);
			}
		}
	}
	else
	{
		std::cout << "Can't find LiDAR" << argv[1] << std::endl;
	}

	return 0;
}
